#include <iostream>
#include <string>
#include <pcl/io/ply_io.h>  
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h> 
#include <pcl/registration/ndt.h> 
#include <pcl/point_types.h> 
#include <pcl/registration/icp.h> 
#include <pcl/visualization/pcl_visualizer.h> 
#include <pcl/console/time.h>   
#include <pcl/filters/voxel_grid.h> 
#include <pcl/common/transforms.h> 
#include <pcl/io/vtk_lib_io.h> 
#include <vector>
#include <pcl/point_cloud.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <pcl/keypoints/sift_keypoint.h>
#include <math.h>

using namespace std;


typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

bool get_point(const string& filename, PointCloudT::Ptr points_cloud)
{
	ifstream fs;
	fs.open(filename.c_str(), ios::binary);
	if (!fs.is_open() || fs.fail())
	{
		fs.close();
		return (false);
	}

	string line;
	vector<string> st;

	while (!fs.eof())
	{
		getline(fs, line);
		// Ignore empty lines
		if (line.empty())
			continue;

		// Tokenize the line
		boost::trim(line);
		boost::split(st, line, boost::is_any_of("\t\r "), boost::token_compress_on);

		if (st.size() != 3)
			continue;

		pcl::PointXYZ point;
		point.x = double(atof(st[0].c_str()));
		point.y = double(atof(st[1].c_str()));
		point.z = double(atof(st[2].c_str()));
		points_cloud->push_back(point);
	}
	fs.close();

	return (true);
}

void print(const Eigen::Matrix4d& matrix)
{
	printf("{");
	printf("\"success\":true,");

	printf("\"matrix\":[");
	printf("%6.3f,%6.3f,%6.3f,%6.3f,", matrix(0, 0), matrix(0, 1), matrix(0, 2), matrix(0, 3));
	printf("%6.3f,%6.3f,%6.3f,%6.3f,", matrix(1, 0), matrix(1, 1), matrix(1, 2), matrix(1, 3));
	printf("%6.3f,%6.3f,%6.3f,%6.3f,", matrix(2, 0), matrix(2, 1), matrix(2, 2), matrix(2, 3));
	printf("%6.3f,%6.3f,%6.3f,%6.3f", (double)0, (double)0, (double)0, (double)1);
	printf("]");

	printf("}");
}

int main(int argc, char* argv[]) {
	if (argc != 20) {
		printf("{\"success\":false}");
		return (0);
	}

	PointCloudT::Ptr cloud_in(new PointCloudT);//��ʼ����
	PointCloudT::Ptr cloud_aim(new PointCloudT);//Ŀ�����
	PointCloudT::Ptr cloud_out(new PointCloudT);//�������


	string filename1 = argv[1];
	string filename2 = argv[2];

	if (!get_point(filename1, cloud_aim) || !get_point(filename2, cloud_in)) {
		printf("{\"success\":false}");
		return (0);
	}

	Eigen::Matrix<double, 4, 4> matrix_rotation;
	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			matrix_rotation(i, j) = atof(argv[4 * i + j + 4]);
		}
	}

	pcl::transformPointCloud(*cloud_in, *cloud_in, matrix_rotation);
	*cloud_out = *cloud_in;

	int iteration = atoi(argv[3]);

	pcl::IterativeClosestPoint<PointT, PointT> icp;   
	icp.setMaximumIterations(iteration);
	icp.setEuclideanFitnessEpsilon(0.0001);
	icp.setInputSource(cloud_out);      
	icp.setInputTarget(cloud_aim);		
	icp.align(*cloud_out);				

	if (!icp.hasConverged())
	{
		printf("{\"success\":false}");
		return (0);
	}
	Eigen::Matrix4d transformation_matrix = icp.getFinalTransformation().cast<double>() * matrix_rotation;
	
	print(transformation_matrix);
	printf("\n");
}